minkowski_difference.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  Copyright (c) 2021-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 Open Source Robotics Foundation 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 COAL_MINKOWSKI_DIFFERENCE_H
#define COAL_MINKOWSKI_DIFFERENCE_H
 
#include "coal/shape/geometric_shapes.h"
#include "coal/math/transform.h"
#include "coal/narrowphase/support_functions.h"
 
namespace coal {
 
namespace details {
 
struct COAL_DLLAPI MinkowskiDiff {
  typedef Eigen::Array<Scalar, 1, 2> Array2;
 
  const ShapeBase* shapes[2];
 
  ShapeSupportData data[2];
 
  Matrix3s oR1;
 
  Vec3s ot1;
 
  Array2 swept_sphere_radius;
 
  bool normalize_support_direction;
 
  typedef void (*GetSupportFunction)(const MinkowskiDiff& minkowskiDiff,
                                     const Vec3s& dir, Vec3s& support0,
                                     Vec3s& support1,
                                     support_func_guess_t& hint,
                                     ShapeSupportData data[2]);
  GetSupportFunction getSupportFunc;
 
  MinkowskiDiff() : normalize_support_direction(false), getSupportFunc(NULL) {}
 
  template <int _SupportOptions = SupportOptions::NoSweptSphere>
  void set(const ShapeBase* shape0, const ShapeBase* shape1);
 
  template <int _SupportOptions = SupportOptions::NoSweptSphere>
  void set(const ShapeBase* shape0, const ShapeBase* shape1,
           const Transform3s& tf0, const Transform3s& tf1);
 
  template <int _SupportOptions = SupportOptions::NoSweptSphere>
  inline Vec3s support0(const Vec3s& dir, int& hint) const {
    return getSupport<_SupportOptions>(shapes[0], dir, hint);
  }
 
  template <int _SupportOptions = SupportOptions::NoSweptSphere>
  inline Vec3s support1(const Vec3s& dir, int& hint) const {
    // clang-format off
    return oR1 * getSupport<_SupportOptions>(shapes[1], oR1.transpose() * dir, hint) + ot1;
    // clang-format on
  }
 
  inline void support(const Vec3s& dir, Vec3s& supp0, Vec3s& supp1,
                      support_func_guess_t& hint) const {
    assert(getSupportFunc != NULL);
    getSupportFunc(*this, dir, supp0, supp1, hint,
                   const_cast<ShapeSupportData*>(data));
  }
};
 
}  // namespace details
 
}  // namespace coal
 
#endif  // COAL_MINKOWSKI_DIFFERENCE_H