intersect.h

/*
 * 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 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