intersect.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2016, Open Source Robotics Foundation
 *  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 FCL_NARROWPHASE_DETAIL_INTERSECT_H
#define FCL_NARROWPHASE_DETAIL_INTERSECT_H
 
#include <limits>
#include "fcl/common/types.h"
#include "fcl/math/geometry.h"
#include "fcl/math/detail/polysolver.h"
 
namespace fcl
{
 
namespace detail
{
 
template <typename S>
class FCL_EXPORT Intersect
{
 
public:
 
  static bool intersect_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
                           const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& p1,
                           S* collision_time, Vector3<S>* p_i, bool useNewton = true);
 
  static bool intersect_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
                           const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1,
                           S* collision_time, Vector3<S>* p_i, bool useNewton = true);
 
  static bool intersect_VF_filtered(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
                                    const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& p1,
                                    S* collision_time, Vector3<S>* p_i, bool useNewton = true);
 
  static bool intersect_EE_filtered(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
                                    const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1,
                                    S* collision_time, Vector3<S>* p_i, bool useNewton = true);
 
  static bool intersect_VE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
                           const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& p1,
                           const Vector3<S>& L);
 
  static bool intersect_Triangle(
      const Vector3<S>& P1,
      const Vector3<S>& P2,
      const Vector3<S>& P3,
      const Vector3<S>& Q1,
      const Vector3<S>& Q2,
      const Vector3<S>& Q3,
      Vector3<S>* contact_points = nullptr,
      unsigned int* num_contact_points = nullptr,
      S* penetration_depth = nullptr,
      Vector3<S>* normal = nullptr);
 
  static bool intersect_Triangle_ODE_style(
      const Vector3<S>& P1,
      const Vector3<S>& P2,
      const Vector3<S>& P3,
      const Vector3<S>& Q1,
      const Vector3<S>& Q2,
      const Vector3<S>& Q3,
      Vector3<S>* contact_points = nullptr,
      unsigned int* num_contact_points = nullptr,
      S* penetration_depth = nullptr,
      Vector3<S>* normal = nullptr);
 
  static bool intersect_Triangle(
      const Vector3<S>& P1,
      const Vector3<S>& P2,
      const Vector3<S>& P3,
      const Vector3<S>& Q1,
      const Vector3<S>& Q2,
      const Vector3<S>& Q3,
      const Matrix3<S>& R,
      const Vector3<S>& T,
      Vector3<S>* contact_points = nullptr,
      unsigned int* num_contact_points = nullptr,
      S* penetration_depth = nullptr,
      Vector3<S>* normal = nullptr);
 
  static bool intersect_Triangle(
      const Vector3<S>& P1,
      const Vector3<S>& P2,
      const Vector3<S>& P3,
      const Vector3<S>& Q1,
      const Vector3<S>& Q2,
      const Vector3<S>& Q3,
      const Transform3<S>& tf,
      Vector3<S>* contact_points = nullptr,
      unsigned int* num_contact_points = nullptr,
      S* penetration_depth = nullptr,
      Vector3<S>* normal = nullptr);
  
private:
 
  static int project6(const Vector3<S>& ax,
                      const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3,
                      const Vector3<S>& q1, const Vector3<S>& q2, const Vector3<S>& q3);
 
  static bool isZero(S v);
 
  static bool solveCubicWithIntervalNewton(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
                                           const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
                                           S& l, S& r, bool bVF, S coeffs[], Vector3<S>* data = nullptr);
 
  static bool insideTriangle(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& c, const Vector3<S>&p);
 
  static bool insideLineSegment(const Vector3<S>& a, const Vector3<S>& b, const Vector3<S>& p);
 
  static bool linelineIntersect(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3, const Vector3<S>& p4,
                                Vector3<S>* pa, Vector3<S>* pb, S* mua, S* mub);
 
  static bool checkRootValidity_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
                                   const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vp,
                                   S t);
 
  static bool checkRootValidity_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
                                   const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
                                   S t, Vector3<S>* q_i = nullptr);
 
  static bool checkRootValidity_VE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
                                   const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp,
                                   S t);
 
  static bool solveSquare(S a, S b, S c,
                          const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
                          const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
                          bool bVF,
                          S* ret);
 
  static bool solveSquare(S a, S b, S c,
                          const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
                          const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp);
 
   
  static void computeCubicCoeff_VF(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& p0,
                                   const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vp,
                                   S* a, S* b, S* c, S* d);
 
  static void computeCubicCoeff_EE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
                                   const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vc, const Vector3<S>& vd,
                                   S* a, S* b, S* c, S* d);
 
  static void computeCubicCoeff_VE(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& p0,
                                   const Vector3<S>& va, const Vector3<S>& vb, const Vector3<S>& vp,
                                   const Vector3<S>& L,
                                   S* a, S* b, S* c);
 
  static bool intersectPreFiltering(const Vector3<S>& a0, const Vector3<S>& b0, const Vector3<S>& c0, const Vector3<S>& d0,
                                    const Vector3<S>& a1, const Vector3<S>& b1, const Vector3<S>& c1, const Vector3<S>& d1);
 
  static S distanceToPlane(const Vector3<S>& n, S t, const Vector3<S>& v);
 
  static bool sameSideOfPlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3, const Vector3<S>& n, S t);
 
  static void clipTriangleByTriangleAndEdgePlanes(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3,
                                                  const Vector3<S>& t1, const Vector3<S>& t2, const Vector3<S>& t3,
                                                  const Vector3<S>& tn, S to,
                                                  Vector3<S> clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
 
  static bool buildTrianglePlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& v3, Vector3<S>* n, S* t);
 
  static bool buildEdgePlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& tn, Vector3<S>* n, S* t);
 
  static void computeDeepestPoints(Vector3<S>* clipped_points, unsigned int num_clipped_points, const Vector3<S>& n, S t, S* penetration_depth, Vector3<S>* deepest_points, unsigned int* num_deepest_points);
 
  static void clipPolygonByPlane(Vector3<S>* polygon_points, unsigned int num_polygon_points, const Vector3<S>& n, S t, Vector3<S> clipped_points[], unsigned int* num_clipped_points);
 
  static void clipSegmentByPlane(const Vector3<S>& v1, const Vector3<S>& v2, const Vector3<S>& n, S t, Vector3<S>* clipped_point);
 
  static S gaussianCDF(S x);
 
  static constexpr S getEpsilon();
  static constexpr S getNearZeroThreshold();
  static constexpr S getCcdResolution();
  static constexpr unsigned int getMaxTriangleClips();
};
 
using Intersectf = Intersect<float>;
using Intersectd = Intersect<double>;
 
} // namespace detail
} // namespace fcl
 
#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h"
 
#endif