shape_shape_contact_patch_func.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2024, INRIA
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
#define HPP_FCL_INTERNAL_SHAPE_SHAPE_CONTACT_PATCH_FUNC_H
 
#include "hpp/fcl/collision_data.h"
#include "hpp/fcl/collision_utility.h"
#include "hpp/fcl/narrowphase/narrowphase.h"
#include "hpp/fcl/contact_patch/contact_patch_solver.h"
#include "hpp/fcl/shape/geometric_shapes_traits.h"
 
namespace hpp {
namespace fcl {
 
template <typename ShapeType1, typename ShapeType2>
struct ComputeShapeShapeContactPatch {
  static void run(const CollisionGeometry* o1, const Transform3f& tf1,
                  const CollisionGeometry* o2, const Transform3f& tf2,
                  const CollisionResult& collision_result,
                  const ContactPatchSolver* csolver,
                  const ContactPatchRequest& request,
                  ContactPatchResult& result) {
    // Note: see specializations for Plane and Halfspace below.
    if (!collision_result.isCollision()) {
      return;
    }
    HPP_FCL_ASSERT(
        result.check(request),
        "The contact patch result and request are incompatible (issue of "
        "contact patch size or maximum number of contact patches). Make sure "
        "result is initialized with request.",
        std::logic_error);
 
    const ShapeType1& s1 = static_cast<const ShapeType1&>(*o1);
    const ShapeType2& s2 = static_cast<const ShapeType2&>(*o2);
    for (size_t i = 0; i < collision_result.numContacts(); ++i) {
      if (i >= request.max_num_patch) {
        break;
      }
      csolver->setSupportGuess(collision_result.cached_support_func_guess);
      const Contact& contact = collision_result.getContact(i);
      ContactPatch& contact_patch = result.getUnusedContactPatch();
      csolver->computePatch(s1, tf1, s2, tf2, contact, contact_patch);
    }
  }
};
 
template <bool InvertShapes, typename OtherShapeType, typename PlaneOrHalfspace>
void computePatchPlaneOrHalfspace(const OtherShapeType& s1,
                                  const Transform3f& tf1,
                                  const PlaneOrHalfspace& s2,
                                  const Transform3f& tf2,
                                  const ContactPatchSolver* csolver,
                                  const Contact& contact,
                                  ContactPatch& contact_patch) {
  HPP_FCL_UNUSED_VARIABLE(s2);
  HPP_FCL_UNUSED_VARIABLE(tf2);
  constructContactPatchFrameFromContact(contact, contact_patch);
  if ((bool)(shape_traits<OtherShapeType>::IsStrictlyConvex)) {
    // Only one point of contact; it has already been computed.
    contact_patch.addPoint(contact.pos);
    return;
  }
 
  // We only need to compute the support set in the direction of the normal.
  // We need to temporarily express the patch in the local frame of shape1.
  SupportSet& support_set = csolver->support_set_shape1;
  support_set.tf.rotation().noalias() =
      tf1.rotation().transpose() * contact_patch.tf.rotation();
  support_set.tf.translation().noalias() =
      tf1.rotation().transpose() *
      (contact_patch.tf.translation() - tf1.translation());
 
  // Note: for now, taking into account swept-sphere radius does not change
  // anything to the support set computations. However it will be used in the
  // future if we want to store the offsets to the support plane for each point
  // in a support set.
  using SupportOptions = details::SupportOptions;
  if (InvertShapes) {
    support_set.direction = ContactPatch::PatchDirection::INVERTED;
    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
        &s1, support_set, csolver->support_guess[1], csolver->supports_data[1],
        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
  } else {
    support_set.direction = ContactPatch::PatchDirection::DEFAULT;
    details::getShapeSupportSet<SupportOptions::WithSweptSphere>(
        &s1, support_set, csolver->support_guess[0], csolver->supports_data[0],
        csolver->num_samples_curved_shapes, csolver->patch_tolerance);
  }
  csolver->getResult(contact, &(support_set.points()), contact_patch);
}
 
#define PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(PlaneOrHspace)          \
  template <typename OtherShapeType>                                          \
  struct ComputeShapeShapeContactPatch<OtherShapeType, PlaneOrHspace> {       \
    static void run(const CollisionGeometry* o1, const Transform3f& tf1,      \
                    const CollisionGeometry* o2, const Transform3f& tf2,      \
                    const CollisionResult& collision_result,                  \
                    const ContactPatchSolver* csolver,                        \
                    const ContactPatchRequest& request,                       \
                    ContactPatchResult& result) {                             \
      if (!collision_result.isCollision()) {                                  \
        return;                                                               \
      }                                                                       \
      HPP_FCL_ASSERT(                                                         \
          result.check(request),                                              \
          "The contact patch result and request are incompatible (issue of "  \
          "contact patch size or maximum number of contact patches). Make "   \
          "sure "                                                             \
          "result is initialized with request.",                              \
          std::logic_error);                                                  \
                                                                              \
      const OtherShapeType& s1 = static_cast<const OtherShapeType&>(*o1);     \
      const PlaneOrHspace& s2 = static_cast<const PlaneOrHspace&>(*o2);       \
      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
        if (i >= request.max_num_patch) {                                     \
          break;                                                              \
        }                                                                     \
        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
        const Contact& contact = collision_result.getContact(i);              \
        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
        computePatchPlaneOrHalfspace<false, OtherShapeType, PlaneOrHspace>(   \
            s1, tf1, s2, tf2, csolver, contact, contact_patch);               \
      }                                                                       \
    }                                                                         \
  };                                                                          \
                                                                              \
  template <typename OtherShapeType>                                          \
  struct ComputeShapeShapeContactPatch<PlaneOrHspace, OtherShapeType> {       \
    static void run(const CollisionGeometry* o1, const Transform3f& tf1,      \
                    const CollisionGeometry* o2, const Transform3f& tf2,      \
                    const CollisionResult& collision_result,                  \
                    const ContactPatchSolver* csolver,                        \
                    const ContactPatchRequest& request,                       \
                    ContactPatchResult& result) {                             \
      if (!collision_result.isCollision()) {                                  \
        return;                                                               \
      }                                                                       \
      HPP_FCL_ASSERT(                                                         \
          result.check(request),                                              \
          "The contact patch result and request are incompatible (issue of "  \
          "contact patch size or maximum number of contact patches). Make "   \
          "sure "                                                             \
          "result is initialized with request.",                              \
          std::logic_error);                                                  \
                                                                              \
      const PlaneOrHspace& s1 = static_cast<const PlaneOrHspace&>(*o1);       \
      const OtherShapeType& s2 = static_cast<const OtherShapeType&>(*o2);     \
      for (size_t i = 0; i < collision_result.numContacts(); ++i) {           \
        if (i >= request.max_num_patch) {                                     \
          break;                                                              \
        }                                                                     \
        csolver->setSupportGuess(collision_result.cached_support_func_guess); \
        const Contact& contact = collision_result.getContact(i);              \
        ContactPatch& contact_patch = result.getUnusedContactPatch();         \
        computePatchPlaneOrHalfspace<true, OtherShapeType, PlaneOrHspace>(    \
            s2, tf2, s1, tf1, csolver, contact, contact_patch);               \
      }                                                                       \
    }                                                                         \
  };
 
PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Plane)
PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH(Halfspace)
 
#define PLANE_HSPACE_CONTACT_PATCH(PlaneOrHspace1, PlaneOrHspace2)           \
  template <>                                                                \
  struct ComputeShapeShapeContactPatch<PlaneOrHspace1, PlaneOrHspace2> {     \
    static void run(const CollisionGeometry* o1, const Transform3f& tf1,     \
                    const CollisionGeometry* o2, const Transform3f& tf2,     \
                    const CollisionResult& collision_result,                 \
                    const ContactPatchSolver* csolver,                       \
                    const ContactPatchRequest& request,                      \
                    ContactPatchResult& result) {                            \
      HPP_FCL_UNUSED_VARIABLE(o1);                                           \
      HPP_FCL_UNUSED_VARIABLE(tf1);                                          \
      HPP_FCL_UNUSED_VARIABLE(o2);                                           \
      HPP_FCL_UNUSED_VARIABLE(tf2);                                          \
      HPP_FCL_UNUSED_VARIABLE(csolver);                                      \
      if (!collision_result.isCollision()) {                                 \
        return;                                                              \
      }                                                                      \
      HPP_FCL_ASSERT(                                                        \
          result.check(request),                                             \
          "The contact patch result and request are incompatible (issue of " \
          "contact patch size or maximum number of contact patches). Make "  \
          "sure "                                                            \
          "result is initialized with request.",                             \
          std::logic_error);                                                 \
                                                                             \
      for (size_t i = 0; i < collision_result.numContacts(); ++i) {          \
        if (i >= request.max_num_patch) {                                    \
          break;                                                             \
        }                                                                    \
        const Contact& contact = collision_result.getContact(i);             \
        ContactPatch& contact_patch = result.getUnusedContactPatch();        \
        constructContactPatchFrameFromContact(contact, contact_patch);       \
        contact_patch.addPoint(contact.pos);                                 \
      }                                                                      \
    }                                                                        \
  };
 
PLANE_HSPACE_CONTACT_PATCH(Plane, Plane)
PLANE_HSPACE_CONTACT_PATCH(Plane, Halfspace)
PLANE_HSPACE_CONTACT_PATCH(Halfspace, Plane)
PLANE_HSPACE_CONTACT_PATCH(Halfspace, Halfspace)
 
#undef PLANE_OR_HSPACE_AND_OTHER_SHAPE_CONTACT_PATCH
#undef PLANE_HSPACE_CONTACT_PATCH
 
template <typename ShapeType1, typename ShapeType2>
void ShapeShapeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
                            const CollisionGeometry* o2, const Transform3f& tf2,
                            const CollisionResult& collision_result,
                            const ContactPatchSolver* csolver,
                            const ContactPatchRequest& request,
                            ContactPatchResult& result) {
  return ComputeShapeShapeContactPatch<ShapeType1, ShapeType2>::run(
      o1, tf1, o2, tf2, collision_result, csolver, request, result);
}
 
}  // namespace fcl
}  // namespace hpp
 
#endif